#include <Arduino.h>
#include <Esp32McpwmMotor.h>
#include <Esp32PcntEncoder.h>
#include <PidController.h>

Esp32McpwmMotor motor;           // 创建一个名为motor的对象，用于控制电机
Esp32PcntEncoder encoders[2];    // 创建一个数组用于存储两个编码器
PidController pid_controller[2]; // 创建PID控制器对象数组
int64_t last_ticks[2];           // 记录上一次读取的计数器数值
int32_t delta_ticks[2];          // 记录两次读取之间的计数器差值
int64_t last_update_time;        // 记录上一次更新时间
float current_speeds[2];         // 记录两个电动机的速度

// put function declarations here:
/**
 * @brief 电机速度控制
 */
void motorSpeedControl();

void setup()
{
    // put your setup code here, to run once:
    Serial.begin(115200);
    encoders[0].init(0, 32, 33);  // 初始化第一个编码器，使用 GPIO 32 和 33 连接
    encoders[1].init(1, 26, 25);  // 初始化第一个编码器，使用 GPIO 26 和 25 连接
    motor.attachMotor(0, 22, 23); // 将电动机0连接到引脚22和引脚23
    motor.attachMotor(1, 12, 13); // 将电动机1连接到引脚12和引脚13
    // 初始化PID控制器参数
    pid_controller[0].update_pid(0.625, 0.125, 0.0);
    pid_controller[1].update_pid(0.625, 0.125, 0.0);
    pid_controller[0].out_limit(-100, 100);
    pid_controller[1].out_limit(-100, 100);
    // 初始化目标速度，单位mm/s,使用毫米防止浮点运算丢失精度
    pid_controller[0].upate_target(100);
    pid_controller[1].upate_target(100);
}

void loop()
{
    // put your main code here, to run repeatedly:
    delay(10);
    motorSpeedControl();
}

// put function definitions here:
void motorSpeedControl()
{
    // 计算时间差
    int64_t current_time = millis();
    int32_t current_ticks0 = encoders[0].getTicks();
    int32_t current_ticks1 = encoders[1].getTicks();
    // 计算时间差
    uint64_t dt = current_time - last_update_time;
    // 计算编码器差值
    delta_ticks[0] = current_ticks0 - last_ticks[0];
    delta_ticks[1] = current_ticks1 - last_ticks[1];
    // 距离比时间计算速度，单位mm/ms乘以1000转换为mm/s，方便PID计算
    current_speeds[0] = float(delta_ticks[0] * 0.1051566) / dt * 1000;
    current_speeds[1] = float(delta_ticks[1] * 0.1051566) / dt * 1000;
    // 更新数据
    last_update_time = current_time;
    last_ticks[0] = current_ticks0;
    last_ticks[1] = current_ticks1;
    // 根据当前速度，更新电机速度值
    motor.updateMotorSpeed(0, pid_controller[0].update(current_speeds[0]));
    motor.updateMotorSpeed(1, pid_controller[1].update(current_speeds[1]));
    // 输出数据
    Serial.printf("speed1=%f mm/s,speed2=%f mm/s\n", current_speeds[0], current_speeds[1]);
}